
#include "ChangeDetection/include/BaselineChangeDetectionControlArchitecture.h"
#include "ChangeDetection/include/ChangeDetectionAgentWorldModel.h"
#include "World/World.h"
#include "ChangeDetection/include/BaselineChangeDetectionController.h"

double BaselineChangeDetectionControlArchitecture::migrationRate;
double BaselineChangeDetectionControlArchitecture::eliteSize;
double BaselineChangeDetectionControlArchitecture::bestRate;
int BaselineChangeDetectionControlArchitecture::migrationType;
int BaselineChangeDetectionControlArchitecture::topologyType;

//BaselineChangeDetectionControlArchitecture::BaselineChangeDetectionControlArchitecture(RobotAgentWorldModel* __wm) : PopulationControlArchitecture(__wm) { //ENABLE FOR HYBRID
BaselineChangeDetectionControlArchitecture::BaselineChangeDetectionControlArchitecture(RobotAgentWorldModel* __wm) : MPOControlArchitecture(__wm) { //ENABLE FOR MU+1
	//setPopulation(new HiveMindInstance()); //ENABLE FOR HYBRID
}

BaselineChangeDetectionControlArchitecture::~BaselineChangeDetectionControlArchitecture() {
	// nothing to do.
}

ControllerPtr BaselineChangeDetectionControlArchitecture::createRandomGenome() {
	/*
	 * Inputs:
	 * Distance sensors
	 * Ditch sensors
	 * organism size
	 * angle to goal
	 * distance to goal
	 * bias node
	 * 
	 * Outputs:
	 * 5x vote for a strategy
	 * organism size
	 * direction
	 * speed
	 */
	//int nbWeights = ((_wm->getDefaultSensors()->getSensorCount() * 2) + 4) * 8;
	int nbWeights = ((_wm->getDefaultSensors()->getSensorCount())) * 5;
	vector<double> weights = vector<double>(nbWeights, 0);
	for (int i = 0; i < nbWeights; i++) {
		weights[i] = Rand::randouble() * 2.0 - 1;
	}

	ControllerPtr p = boost::make_shared<BaselineChangeDetectionController>(weights);
    return p;
}

double BaselineChangeDetectionControlArchitecture::calculateStepFitness() {
	ChangeDetectionAgentWorldModel* worldModel = dynamic_cast<ChangeDetectionAgentWorldModel*> (_wm);

	double distance = worldModel->distanceTraveledLastIteration;
	double fitness = distance;
	//std::cout << fitness << std::endl;

	return fitness;
}

//Intentive method below, but wasn't working as expected due to where locations are updated
/*double BaselineChangeDetectionControlArchitecture::calculateStepFitness() {
	ChangeDetectionAgentWorldModel* worldModel = dynamic_cast<ChangeDetectionAgentWorldModel*> (_wm);

	double distance;
	if(worldModel->fitnessWrapAround){
		//std::cerr << "WRAAAP!!!!!"<<std::endl;
		worldModel->fitnessWrapAround = false;
		distance = (worldModel->getPosition().x -(gAreaWidth/13/2)) + ( worldModel->xCoordinateBeforeWrap - worldModel->xStart ); //maybe +1
	}else{
		distance = worldModel->getPosition().x - worldModel->xStart;
	}
	double distance = worldModel->getPosition().x - worldModel->xStart;
	double fitness = distance;
	std::cout << fitness << std::endl;

	return fitness;
}*/

/*
double BaselineChangeDetectionControlArchitecture::calculateStepFitness() {
	ChangeDetectionAgentWorldModel* worldModel = dynamic_cast<ChangeDetectionAgentWorldModel*> (_wm);

	double distance = worldModel->getPosition().x - worldModel->xStart;
	double fitness = distance;

	return fitness;
}
*/
